Generalized Nonlinear Complementary 
Attitude Filter 

Kenneth J. Jensen^ 
Makani Power Inc., Alameda, CA, 9^501 

I. Introduction 

The muhiphcative extended Kalman fiher (MEKF) is a proven attitude estimation technique[l, 
2], that is the de facto standard for low-cost inertial measurement units (IMUs). However, the 
MEKF does have its drawbacks. It is computationahy expensive, difhcuh to tune, and potentiahy 
subject to divergence due to numerical errors. This has lead to the development of alternate attitude 
estimation algorithms such as the unscented Kalman filter [3, 4], generalized MEKF [5], invariant 
extended Kalman filter [6], particle filters [7, 8], and a host of nonlinear observers [9-16]. These 
techniques and others are summarized in a review article [17] by Crassidis et at. 

A relatively new and popular estimation technique is Mahony's nonlinear complementary filter 
[13; 18[. This filter boasts computational efficiency, a small number of easily tuned, intuitive pa- 
rameters, and a proof of almost global asymptotic stability. Moreover, this filter has been shown to 
perform similarly to the traditional MEKF [19[. 

This work describes a new attitude estimation technique that is a generalization of Mahony's 
nonlinear complementary filter. Interestingly, this generalized attitude filter shares a close mathe- 
matical relationship with the MEKF, and indeed both the MEKF without gyro bias correction and 
the constant gain MEKF with gyro bias correction may be viewed as special cases of the generalized 
attitude filter. 
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II. Attitude Estimation 

The attitude estimation problem consists of combining measurements from various potentially 
imperfect sensors located onboard an object {e.g. vehicle, aircraft, etc..) into an accurate estimate 
of the attitude of the object. There are many versions of this problem, which vary in parameters 
such as the attitude representation, object dynamics, and measurement models. 

The two primary attitude representations used in this work are the direction cosine matrices 
for a global representation and the Euler vector for small changes in attitude near the identity. 
The global attitude, meaning the rotation from the inertial frame to the frame that rotates with the 
object {i.e. body frame), is denoted by C. From another viewpoint, C represents the transformation 
from body to inertial coordinates: Cv = v^. (As described in the Appendix, the notation denotes 
vectors or matrices in inertial coordinates; otherwise, vectors or matrices should be assumed to be 
in body coordinates.) Small changes in attitude are denoted by C or by the Euler vector x where 
C^I + x^. 

No assumptions are made as to the dynamics of the rotating object. Rather, the process 
equations are based on the kinematics of the rotation group itself. Specifically, the attitude C of an 
object with angular velocity w, measured in the body frame, evolves according to 

C = Cwx (1) 

The generality of the kinematics allows these results to apply to a wide range of attitude estimation 
problems. 

Finally, this work assumes a typical set of measurements, including an angular rate measurement 
from a 3-axis rate gyro and a set of vector measurements from, for example, a 3-axis magnetometer 
measuring Earth's magnetic field and a 3-axis accelerometer measuring, with appropriate filtering. 
Earth's gravity vector. The angular rate measurement, w, is corrupted from the true angular rate, 
ui, by additive zero- mean white noise, 1]^^, and a slowly varying bias error, 5, which is driven by a 
white noise process, rjf^. Similarly, the vector measurements, Vn, of inertial vectors in the body frame 
are corrupted from the true vectors, C^v J by additive white noise, rj^^ . Finally, it is assumed that 
the vector measurements have all been normalized to be unit vectors. To summarize, the angular 
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rate and vector measurement models used in this work are 

w = u; + 6 + J7^, b = r]^ (2a) 
Vn = C^vl + r,,^, \vl\ = l (2b) 

The kinematics (1) and measurement model (2) completely specify the attitude estimation problem. 

III. Multiplicative Extended Kalman Filter 

The MEKF applies the extended Kalman filter (EKF) formalism [20] to the attitude estima- 
tion problem. Although the MEKF typically uses quaternions as its attitude representation, it is 
reformulated here in terms of direction cosine matrices and Euler vectors to facilitate comparison 
with the nonlinear compleineiitary filter. After the derivation of the process equations for the Euler 
vector, this description of the MEKF closely follows the form given in [2]. 

The true attitude C, estimated attitude Cref , and error in attitude C are related according to 

C = ClfC = I + x^+... (3) 

where C is linearized about the identity with the three-component Euler vector, x, as discussed 
earlier. The process equations for the Euler vector are determined by the kinematics of the rotation 
group (1). These kinematics apply equally well to the reference attitude: Cref = Cref('''ref)x, where 
Wref is the angular velocity of the reference attitude. Thus, 

C = CjgfC + Cj^fC 

= -(a)ref)xC' + C''^x 
= (W - Wref)x 

+ ja^x, ^(w - c<;ref)x I + ••• 

The symmetric terms are second or higher order in ccx • Retaining only the first-order anti-symmetric 
terms of (3) yields the equation for the evolution of the Euler vector: 

X^U) - Wref - ^ (l^ + <^ref ) X X. (4) 

It is now possible to apply the EKF formalism to form an estimate, x, of this attitude error 
vector. Following the typical procedure for the MEKF, Wref is set such that (i) = 0. Thus, Wref 



1/ 

a^x, ^('^ + '^ref)x 



represents an estimate of the error between the current reference attitude Crei and the true attitude, 
C, which may now be estimated simply by integrating (1) with w = u>ref- Finally, similar to the 
results in [2], the defining equations for the continuous-time MEKF are: 



Wref = 0L3-b + Pa^ (■»„ X f ref „) 

n 



(5a) 
(5b) 



where Wrefn = ^ref^n ^^'^ measurement noise covariance based on rj^^. Also, the covari- 

ance matrix, P, has been partitioned into 3x3 blocks 



P = 



Pa Pc 



with Pa representing the attitude covariances and Pb representing the bias covariances. The covari- 
ance matrix evolves according to the standard Ricatti equation, which becomes 



P = 



[P„,w'J-2P,(Pe) -Uj'^Pc-Pb 

Pju;; - Pt 
all 



+ 



all 



(6) 



P^{Vrefn)lPa Pj {v..in)lP. 

where u>' = \(<jJ + Wref — b) and and a1 are process noise covariances based on r}^ and 77^ 
respectively. 

For reasons discussed in Section IV below, the full continuous-time MEKF is not a special case 
of the generalized attitude filter. However, two important special cases of the MEKF are related 
to the generalized attitude filter. These are the bias-free MEKF and the constant gain MEKF. 
The bias- free MEKF simply assumes that the gyro measurement has no bias term, 6 = 0. The 
constant gain MEKF, which is often used to reduce computational load, retains the gyro bias term 
but assumes that the covariance matrix is constant at the value it would approach as t — ^ 00 with 



w = 0. 
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IV. Generalized Nonlinear Complementary Attitude Filter 

The nonlinear complementary filters are a set of attitude estimators inspired by traditional 
linear complementary filters. Like a linear complementary filter, the nonlinear complementary filters 
combine low frequency attitude information from a set of vector measurements with high frequency 
attitude information from rate gyros. An interesting feature of this family of estimators is the 
ability to prove the almost global asymptotic stability of the estimator. This work demonstrates 
how the stability proof may be extended to a generalization of the nonlinear complementary filters. 
Moreover, it is shown that the deterministic [21] forms of the MEKF without gyro biases and the 
constant gain MEKF with gyro biases are special cases of this generalized attitude filter, and thus, 
it is proven that these forms of the MEKF are almost globally asymptotically stable. 

The form for Mahony's explicit complementary filter [13] is 

C = C (u -b + fcpWerr) , fcp > (7a) 
6 = — fc/Werr, fc/ > (7b) 

Werr = ^ knVn X t)„, fc„ > (7c) 

n 

where t)„ = C'^v^. This filter may be generalized simply by replacing the positive constant scalar 
gains, kp and kj, by potentially time-varying positive-definite matrix gains, Kp and Kj. 

d = C(u-b + ii'pC4;err) , -ffp > (8a) 
b = -KiU)err, Ki>0 (8b) 

Werr = ^ k„Vn X ■&„, A;„ > (8c) 

n 

At this point, it is interesting to note that the (u) — b) + Kpuie„ term from this filter is identical to 
the Wref term (5a) from the MEKF, including the positive-definite nature of the covariance matrix. 
Moreover, the equation for 6 here is similar to (5b), the equation for the derivative of the bias 
estimate in the MEKF. However, an important distinction is that the integral matrix gain, — Pj, 
in the MEKF is not necessarily positive definite. 

It is easier to analyze the stability properties of the error dynamics of the filter rather than the 
filter itself. Thus, the following definitions for the errors between the true and estimated attitudes 
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and biases are introduced. 

C = C^C (9a) 
6 = 6-6 (9b) 

The definition for the attitude error, C, here is the same as the definition used in the MEKF. By 
these definitions, the filter has converged to true attitude and bias when {C, 6) = (/, 0). 

Combining the definitions for the error terms (9), the definition of the generahzed attitude 
filter (8), and the measurement models (2) with no noise terms yields the equations for the error 
dynamics: 

{b + KpuJ^„)^C (10a) 

(10b) 

X CVn (lOc) 

As demonstrated in the proof below, the equilibria of the error dynamics, denoted (7*, are 
determined by the inertial vectors and the weights kn, or more precisely by the matrix M: 

M = C^M^C with = Y,knVn{vlf. (11) 

n 

The stability proof relies on M being positive semi-definite with distinct eigenvalues, which as shown 
in [13], is true if there are at least two non-parallel measurement vectors. Intuitively, the equilibria 
occur when the attitude error, C, is the identity rotation [i.e. the filter has converged) or a rotation of 
TT rad about one of the principle axes of M. More specifically, the equilibria occur when Fa{CM) = 0, 
which according to Lemma 1 below, implies that the equilibria are given by = UDiU^ where 
Dq = I, Di = diag(l,-l,-l), D2 = diag(-l,l,-l), and D3 = diag(-l, -1, 1) for M = UMJ'^ 
with diagonal A and orthogonal U. The proof below first demonstrates that the equilibria indeed 
occur at C*i, and then analyzes the stability properties of each equilibrium. 

The following proof of the stability characteristics of the generalized attitude filter is substan- 
tially similar to the proofs given in [13]. The primary difference is the extension of the proof to 
handle potentially time-varying matrix gains rather than constant scalar gains. Some results from 
[13], such as the following lemma, transfer with no modification, and consequently are simply re- 
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(7 = [C,u>x]- 

6 = -ftT/Werr 
Werr = ^ ] knVn 



stated here. The stabihty properties of the generalized attitude filter are now characterized with 
the following theorems. 

Lemma 1. Suppose C G S0{3) and M is positive semi-definite with distinct eigenvalues and 
decomposition M = UAU^ for orthogonal U and diagonal A. Then, Fa{CM) = if and only 
if C = C^i = UDiU^ where Dq = I, Di = diag(l, -1, -1), D2 = diag(-l, 1, -1), and D3 = 
diag(-l,-l,l). [13] 

Theorem 1 (Stability of the generalized attitude filter). Consider the error dynamics described 
by (10). Suppose that Kp, Kj, and M are positive definite, Kj is positive semi-definite, and 
kn > 0. Further, suppose that Kp and Kj are upper and lower bounded by positive constants; 
Kp, Kj, Kj, and U3 are bounded; and M has distinct eigenvalues. Then, the equilibrium point 
{C, b) = (/, 0) of the error dynamics is asymptotically stable with a domain of attraction V = 
{iC,b) e 50(3) xR3-{(C'*i,0)|i= 1,2,3}} and is locally exponentially stable. 

Proof. Consider the Lyapunov function candidate 

v = ^K- tr(CM) + Ib^K^^b (12) 

n 

Following the standard procedure, the time derivative of v is calculated: 

V = -ti(CM + CM) + b^KT^h + H^^KT^b 

2 at 

= -tr ([C'M,a;x] - {b + Kpuj,,,)^Cm) 
+ b'u:,„-^fKj^kiKj^b 

= tr (ib + KpU;err)xPa(CM)) - -tr (bx ('*'err)x) 

- ^fKj'kiKj'i, 

= tr ((Jrpa;err)xFa(C'M)) - Ui^Kj^kiKY^b (13) 

where the identity (24c) from the Appendix and the result (u>err)x = 2Pa(C'M), also obtainable 
from the identities (24), were used in the simplification. As Kj > 0, the second term in (13) is 
negative semi-definite. To show that the first term is also negative semi-definite, it is useful to 
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rewrite KpUerv in terms of the measurement vectors 

{KpU)err)x = I ^k„{QVn) X {QCv„) j 

= ^A:„ {QCVniVnfQ'^ - QVniCVnfQ^) 
n 

= 2QVa{CM)Q'^ 

where Q = ±\/det KpKp^ is a positive or negative definite matrix. Then, using the fact that Q 
has a decomposition Q = S'^AS for orthogonal S and diagonal A: 

tr((ifpWerr)xPa((5M)) 

= 2tr (QP„(CM)g^P„(CM)) 

= 2tr ((S'^A5)Pa(C'M)(6'^A5)Pa(C'M)) 

= 2tr (^A{SFa{CM)S'^)A{SFa{CM)S'^)^ 

= 2 tr ((AP„(6'(7M6''^))2) < (14) 

This results in the trace of the square of the product of a diagonal matrix and an antisymmetric 
matrix, which is easily shown to be negative semi-definite. Thus, combining (14) and (13), it is 
shown that i) <0. Finally, with the assumptions on the boundedness of Kp, Kj, Kp, Kj, Kj, and 
U}, it is straightforward to show that v, given by 

V =4tr (qP„(CM) (oP„(C'M) 

+QP„([CM,u;x] - 2QP„((7M)Q^(7M))) 

r'T d IfT d^ ^^_ir 

+ ^ dt^^ ^+2^ dt^^^ ^ 

is bounded. Therefore, Barbalat's lemma [22] implies that i) and thus Pa (CM) tend asymptotically 

to zero. According to Lemma 1, the attitude error, C must approach one of the equilibria C^j. 

Moreover, Fa{CM) — >■ implies that u>err — >■ 0, and thus, after substituting the relation C'j.j = 

[(7*4, Wx] into (10), it is shown that b must also tend asymptotically to zero. 

To demonstrate the stability characteristics of the various equilibria, the system is linearized 
about each C*i. Because the equilibria attitude errors are constant in the inertial frame {i.e. 

= 0), it is easiest to conduct the linearization in the inertial frame. The error dynamics (10) 



expressed in the inertial frame are 



71- 



6^ = wt^6 +Kfu\ 



I. ,x 

err 



(15a) 
(15b) 



where C-'' = CCC^ as described in the Appendix. Let C-^ « C^^{l + x-f,) and 6 w —y^. First, the 

measurement error vector is Unearized: 

«^fc„t;Jx(75(l + x5)t;; 

n 

= J2 knvl X C^vl - J2 knvl X C^ivD^X^ 

n n 

n 

= (tr(M^)J - M^) C^^x^ 



AM 



(16) 



where Af = C% (tr(M^)7 - M^) and = Cja;^. Substituting (16) into (15) with the Unearized 



C and 6 results in 







A? 













x-i 



(17) 



To demonstrate the instabiUty of the equiUbria (7*, for i = 1,2,3, it is necessary to show that 
a trajectory starting arbitrarily close to C*, {i.e. \^'^\ arbitrarily close to zero with ^"^ = {xf,y^)) 
must eventually diverge from the compact set, which contains the equilibrium, defined by 1^"^! < r 
for some r chosen such that the linearization is still valid. Consider the cost function 



s^lixffAfxf+liy^fiKfr^y^ 



(18) 
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The time derivative of s is given by 



s =-( - (xfY {AfY {K-Y A-x- + {y^Y 



{xff{AfnKff{Kf)-'y^ + {y^nKfr'ulv^ 

iy^f{Kf)-'KfAfxf + {xffAfxf 



{xffiAffK^pAfxf 
l{y^f{Kf)-\krf{Kf)-'y^ 



(19) 



which is negative definite as Kj and thus (-K'/)'^ are positive definite. For i = 1, 2, 3, At has at least 
one negative eigenvalue, and thus there is some ^J, with magnitude arbitrarily close to zero, for 
which s($o ) < 0- As s < and because r was chosen such that the linearization is valid, trajectories 
^"^(f) starting at must eventually pass through the sphere with radius r. Thus, these equilibria 
are unstable. 

The local exponential stability of the equilibrium point {C, b) = (J, 0) is now proven. Take 
the (5*0 = I case of the linearized system (17) and return the system to body coordinates. The 
simplified linearized system is 



^ = Bi, B 



(20) 



-KpAo -u>x I 
-KjAo 

where Aq = tv{M)I — M is positive definite. To prove the exponential stability of the equilibrium 
point ^ = (0,0) of the linearized system (20), consider the Lyapunov function candidate 



(21) 



Aq -uAq 
-aAo KJ^ 

where a is chosen such that P is positive definite, or more specifically such that o^Aq < KJ^. The 
derivative of w along the trajectories of the system is 



w 



f {PB + B'^P + P) 4 = -fQi 
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where 



Q = -{PB + B^P + P) 

2Aq{Kp - aKi)Ao -a{AoKpAo - Aqoj^) 
-aiAoKpAo+oj^Ao) 2aAQ + Kj^KiKj^ 
Using the Schur complement condition for positive definiteness, Q is positive definite exactly when 
Kp > aKj and 

2aAo + Kj^kiKj^ 

> ^a'^{AoKp + oj^){Kp- aKi)-\KpAo - Wx) 

With if/ > 0, it is straightforward to show that there exists some a > such that P > and Q > 0. 
Therefore, w is upper bounded by a negative constant and the linearized system is exponentially 
stable. 

Together, the results on the asymptotic convergence of {C,b) to ((7*8,0), the instability of the 
C*i, C*2, and (7*3 equilibria, and the stability of the (7*o equilibrium show the asymptotic stability 
of (7, 0) with domain of attraction V. □ 

A direct corollary of Theorem 1 is that the deterministic [21] bias- free MEKF is almost globally 
asymptotically stable. Simply identify Kp = Pa and set the bias error in the proof to zero. It is 
easy to show, given the boundedness of U3, that Pa and Pa meet the boundedness requirements of 
Kp and Kp for the theorem. The proof then proceeds without modification. Another interesting 
special case is that of the deterministic constant gain MEKF. The following theorem demonstrates 
that the deterministic constant gain MEKF is almost globally asymptotically stable as well. 

Theorem 2. The deterministic constant gain MEKF defined by (5) with Pa ~ Pa{oo) and Pc = 
Pc(oo) is asymptotically stable with a domain of attraction V defined in Theorem 1. 

Proof. The proof proceeds by demonstrating that the constant gain MEKF is a special case of 
the generalized attitude filter. It has already been shown that the forms for the MEKF and the 
generalized attitude filters are similar. All that remains to be shown is the positive definite nature 
of the matrix gains. Pa and —Pc in the constant gain MEKF. 
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With w = and assuming the covariance matrix converges then equations for the matrix gains 

are 

Pa = -2P,(Pe) + (^ll - PaAoPa= (22a) 

Pb = all - pJAqP, = {22b) 

Pj = -Pb - PjAoPa = (22c) 

where as in Theorem 1, Aq = tr(M)/ — M. The combining of (22b) and (22c) yields 

Pe = -<7lPaPb-'- (23) 

Substitution into (22a) results in 

PaPb^' + Pb'Pa = -'^t^'^ll + P^ 

from which it is evident that Pa and Pb are simultaneously diagonalizable. Thus, as Pa and Pb are 
positive definite since the entire covariance matrix is positive definite, (23) shows that Pc is negative 
definite. 

Identifying Kp = Pa and Kj = — Pc results in the equations for the generalized attitude filter, 
which by Theorem 1 is asymptotically stable with a domain of attraction V. □ 

V. Conclusions 

This paper introduces a very general class of attitude estimators that contains Mahony's explicit 
nonlinear complementary filter, the bias- free multiplicative extended Kalman filter (MEKF), and 
the constant-gain MEKF as special cases. This generalized attitude filter is a modification of 
Mahony's filter that simply replaces the constant scalar gains by potentially time-varying matrix 
gains. The stability proofs developed for Mahony's filters extend naturally to the generalized filter. 
Thus, it is possible to prove the almost global asymptotic stability of this general class of filters, 
and consequently provide proof of the almost global asymptotic stability of a few special cases of 
the MEKF. This generalized attitude filter gives the filter designer an enormous space to tune and 
optimize the filter, while ensuring stability. It is important to note that this generalized attitude 
filter does not include as a special case the full MEKF, which may not necessarily have a positive 
definite integral gain. 
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Table 1 Notation 

Notation Interpretation 

a True value 

a Estimated value 

a Measured value 

a Error between estimated and true value 

(o) Time-averaged expectation value 

A Capital letters indicate matrices 

A Left multiply error: AA^ = 

A, Equilibrium point 

A^ Matrix in inertial frame: A^ = CAC"^ 

Ps(A), Va{A) Symmetric or anti-symmetric projection of A 

[A, B\, {A, B} Commutator or anti-commutator of A and B 

X Bold lowercase letters indicate vectors 

x"^ Vector in inertial frame: x"^ = Cx 

X X Matrix equivalent form of the cross product 



Appendix 

There is an unfortunate abundance of notation employed in this paper, which results both from 
the nature of the work as well as an attempt to display the results from the MEKF papers and the 
nonlinear complementary paper in a compatible fashion. The notation most closely follows that in 
Mahony's paper [13], with only minor changes made to avoid conflicts with the other works. Table 
1 describes the major elements of the notation. 

This work also makes heavy use of a few identities and definitions described below. The matrix 
equivalent form of the cross product, also known as the cross operator, is defined by 

—X3 X2 
= Xs —Xi 
—X2 X\ 
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The symmetric and anti-symmetric matrix operators are defined as 

The commutator and anti-commutator are defined as 

[A, B] = AB- BA 

{A, B} = AB + BA. 

Some useful identities for manipulating cross products and cross operators are 

(x X y)x = [xx,yx] (24a) 
QxxQy={detQ)Q-\xxy) (24b) 
a;^y = -itr (ajxyx) (24c) 

xxyx= y^^ - y^^i (24d) 

where Q in (24b) must be positive or negative definite. The following identities are useful for 
expanding or contracting matrices with the cross operator: 

¥a{AxxB) = \{Cx)x (25a) 
SxxS'^ = {Sx)x (25b) 
AxxA = {Dx)x (25c) 

where A = SAaS^ , B = SAbS^, C = SAqS'^, and D = SAdS^ with S e 50(3) and where 

Aa = diag(Ai, A2, A3) 
As =diag(Ai,A2,A3) 

Ac = diag(A2A3 + A2A3, A1A3 + A;A3, A1A2 + A'iA2) 
Ad = diag(A2A3,AiA3, A1A2). 

Identities (25b) and (25c) are just special cases of the first identity (25a). 
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